/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 障碍层文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/obstacle_layer.h>

using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace costmap_2d {

void ObstacleLayer::onInitialize() {
  rolling_window_ = layered_costmap_->isRolling();

  if (layered_costmap_->isTrackingUnknown()) {
    default_value_ = NO_INFORMATION;
  } else {
    default_value_ = FREE_SPACE;
  }
  ObstacleLayer::matchSize();
  current_ = true;

  // transform_tolerance topics_string 算是获取数据要的参数的一部分
  bool clearing, marking;
  clearing = (*configNode_).getValueOfKey<bool>("clearing", false);
  marking = (*configNode_).getValueOfKey<bool>("marking", false);
  footprint_clearing_enabled_ =
      (*configNode_).getValueOfKey<bool>("footprint_clearing_enabled", true);
  observation_buffers_ = boost::shared_ptr<ObservationBuffer>(
      new costmap_2d::ObservationBuffer(configNode_));

  if ((*configNode_).getValueOfKey<bool>("obstacle_use_maximum", true)) {
    combination_method_ = 1;
  } else {
    combination_method_ = 0;
  }

  // check if we'll add this buffer to our marking observation buffers
  if (marking) {
    marking_buffers_ = boost::shared_ptr<ObservationBuffer>(
        new costmap_2d::ObservationBuffer(configNode_));
    marking_buffers_ = observation_buffers_;
  }
  // check if we'll also add this buffer to our clearing observation buffers
  if (clearing) {
    clearing_buffers_ = boost::shared_ptr<ObservationBuffer>(
        new costmap_2d::ObservationBuffer(configNode_));
    clearing_buffers_ = observation_buffers_;
  }

  // 创建获取数据的线程，更新observation_buffer
  std::thread collectObservationThread(&ObstacleLayer::getDatafromCollector,
                                       this);
  std::thread collectOdomThread(&ObstacleLayer::getOdomfromCollector, this);
  collectObservationThread.detach();
  collectOdomThread.detach();
}

ObstacleLayer::~ObstacleLayer() {}
void ObstacleLayer::getDatafromCollector() {
  // buffer the point cloud
  while (true) {
    costmap_2d::PointCloud* pts = new costmap_2d::PointCloud();
    data_collector_->getPts(*pts);
    observation_buffers_->lock();
    observation_buffers_->bufferCloud(pts, odomTransformEigen_, origin_);
    observation_buffers_->unlock();
    sleep(0.2);
  }
}

void ObstacleLayer::getOdomfromCollector() {
  while (true) {
    costmap_2d::Pose origin;
    data_collector_->getOdom(origin);

    Eigen::Transform<float, 3, Eigen::Isometry> t0 =
        Eigen::Translation3f(0, 0, 0) * Eigen::Quaternion<float>(1, 0, 0, 0);
    Eigen::Transform<float, 3, Eigen::Isometry> t =
        Eigen::Translation3f(origin.position.x, origin.position.y,
                             origin.position.z) *
        Eigen::Quaternion<float>(origin.orientation.w, origin.orientation.x,
                                 origin.orientation.y, origin.orientation.z);

    t = t0 * t;
    odomTransformEigen_ = t;
    origin_.x = origin.position.x;
    origin_.y = origin.position.y;
  }
}

void ObstacleLayer::updateBounds(double robot_x, double robot_y,
                                 double robot_yaw, double* min_x, double* min_y,
                                 double* max_x, double* max_y) {
  if (rolling_window_) {
    updateOrigin(robot_x - getSizeInMetersX() / 2,
                 robot_y - getSizeInMetersY() / 2);
  }
  if (!enabled_) {
    return;
  }
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<Observation> observations, clearing_observations;
  // get the marking observations
  current = current && getMarkingObservations(observations);

  // get the clearing observations
  current = current && getClearingObservations(clearing_observations);

  // update the global current status
  current_ = current;
  // raytrace freespace
  // 清除free的区域是吧
  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }
  // place the new obstacles into a priority queue... each with a priority of
  // zero to begin with

  for (std::vector<Observation>::const_iterator it = observations.begin();
       it != observations.end(); ++it) {
    const Observation& obs = *it;

    const costmap_2d::PointCloud& cloud = *(obs.cloud_);

    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
    for (int i = 0; i < cloud.points.size(); i++) {
      double px = cloud.points.at(i).x, py = cloud.points.at(i).y,
             pz = cloud.points.at(i).z;

      // if the obstacle is too high or too far away from the robot we won't add
      // it
      if (pz > max_obstacle_height_) {
        continue;
      }

      // compute the squared distance from the hitpoint to the pointcloud's
      // origin
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) +
                       (py - obs.origin_.y) * (py - obs.origin_.y);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range) {
        continue;
      }

      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my)) {
        continue;
      }
      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::updateFootprint(double robot_x, double robot_y,
                                    double robot_yaw, double* min_x,
                                    double* min_y, double* max_x,
                                    double* max_y) {
  // 转换出footprint 然后 跟边界进行裁剪。
  if (!footprint_clearing_enabled_) {
    return;
  }
  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(),
                     transformed_footprint_);

  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x,
          min_y, max_x, max_y);
  }
}

void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
                                int min_j, int max_i, int max_j) {
  if (!enabled_) {
    return;
  }

  if (footprint_clearing_enabled_) {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }
  switch (combination_method_) {
    case 0:  // Overwrite
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
}

bool ObstacleLayer::getMarkingObservations(
    std::vector<Observation>& marking_observations) const {
  bool current = true;
  // get the marking observations

  marking_buffers_->lock();
  marking_buffers_->getObservations(marking_observations);
  current = marking_buffers_->isCurrent() && current;
  marking_buffers_->unlock();

  return current;
}

bool ObstacleLayer::getClearingObservations(
    std::vector<Observation>& clearing_observations) const {
  bool current = true;
  // get the marking observations

  clearing_buffers_->lock();
  clearing_buffers_->getObservations(clearing_observations);
  current = clearing_buffers_->isCurrent() && current;
  clearing_buffers_->unlock();

  return current;
}

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation,
                                      double* min_x, double* min_y,
                                      double* max_x, double* max_y) {
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  const costmap_2d::PointCloud& cloud = *(clearing_observation.cloud_);

  // get the map coordinates of the origin of the sensor
  unsigned int x0, y0;
  if (!worldToMap(ox, oy, x0, y0)) {
    return;
  }

  // we can pre-compute the enpoints of the map outside of the inner loop...
  // we'll need these later
  double origin_x = origin_x_, origin_y = origin_y_;
  double map_end_x = origin_x + size_x_ * resolution_;
  double map_end_y = origin_y + size_y_ * resolution_;

  touch(ox, oy, min_x, min_y, max_x, max_y);

  // for each point in the cloud, we want to trace a line from the origin and
  // clear obstacles along it
  for (int i = 0; i < cloud.points.size(); i++) {
    double wx = cloud.points.at(i).x;
    double wy = cloud.points.at(i).y;

    // now we also need to make sure that the enpoint we're raytracing
    // to isn't off the costmap and scale if necessary
    double a = wx - ox;
    double b = wy - oy;

    // the minimum value to raytrace from is the origin
    if (wx < origin_x) {
      double t = (origin_x - ox) / a;
      wx = origin_x;
      wy = oy + b * t;
    }
    if (wy < origin_y) {
      double t = (origin_y - oy) / b;
      wx = ox + a * t;
      wy = origin_y;
    }
    // the maximum value to raytrace to is the end of the map
    if (wx > map_end_x) {
      double t = (map_end_x - ox) / a;
      wx = map_end_x - .001;
      wy = oy + b * t;
    }
    if (wy > map_end_y) {
      double t = (map_end_y - oy) / b;
      wx = ox + a * t;
      wy = map_end_y - .001;
    }

    // now that the vector is scaled correctly... we'll get the map coordinates
    // of its endpoint
    unsigned int x1, y1;

    // check for legality just in case
    if (!worldToMap(wx, wy, x1, y1)) {
      continue;
    }
    unsigned int cell_raytrace_range =
        cellDistance(clearing_observation.raytrace_range_);
    MarkCell marker(costmap_, FREE_SPACE);
    // and finally... we can execute our trace to clear obstacles along that
    // line
    //  在这里更新costmap_
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_,
                         min_x, min_y, max_x, max_y);
  }
}

void ObstacleLayer::activate() {}

void ObstacleLayer::deactivate() {}

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx,
                                         double wy, double range, double* min_x,
                                         double* min_y, double* max_x,
                                         double* max_y) {
  double dx = wx - ox, dy = wy - oy;
  double full_distance = hypot(dx, dy);
  double scale = std::min(1.0, range / full_distance);
  double ex = ox + dx * scale, ey = oy + dy * scale;
  touch(ex, ey, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::reset() {}

}  // namespace costmap_2d
